#include "parseCmdMCode.hpp"
#include "report.hpp"
#include "../Misc/misc.hpp"
#include "../TempControl/powerDrive.hpp"
#include "../Global/globalVars.hpp"
//#include "../TempControl/tempCtrlModel.hpp"
#include "../main/tempCtrlService.hpp"
#include "../LedControl/ledControl.h"

void printMCodeReply(const char* content);
void processDeactivateAll();
void printDeviceInfo();
void decodeSetPlateTemp(const char* cmd);

bool parseCmdMCode(const char* cmdIn)
{
	if (StartWith(cmdIn, "?"))
	{
		printMCodeReply(Report::reportState());
		return true;
	}
	// M104 MODE,TEMPERATURE,RAMP_RATE,DURATION
	// MODE: "cool"/"heat"/"dual"
	// TEMPERATURE: 95.5
	// RAMP_RATE: 4
	// DURATION: seconds, -1 for no stop
	// M104 heat,32,2,0\r\n
	// M104 cool,10,2,0\r\n
	if (StartWith(cmdIn, "M104 "))
	{
		//    'SET_PLATE_TEMP': 'M104',
		decodeSetPlateTemp(cmdIn + 5);
		return true;
	}
	if (StartWith(cmdIn, "M18"))
	{
		//    'DEACTIVATE_ALL': 'M18',
		processDeactivateAll();
		return true;
	}
	if (StartWith(cmdIn, "M105 "))
	{
		//    'GET_PLATE_TEMP': 'M105',
		printMCodeReply(Report::reportState());
		return true;
	}
	if (StartWith(cmdIn, "M126 "))
	{
		//    'OPEN_LID': 'M126',
		printMCodeReply("OPEN_LID not implemented\r\n");
		return true;
	}
	if (StartWith(cmdIn, "M127 "))
	{
		//    'CLOSE_LID': 'M127',
		printMCodeReply("CLOSE_LID not implemented\r\n");
		return true;
	}
	if (StartWith(cmdIn, "M119 "))
	{
		//    'GET_LID_STATUS': 'M119',
		printMCodeReply("GET_LID_STATUS not implemented\r\n");
		return true;
	}
	if (StartWith(cmdIn, "M140 "))
	{
		//    'SET_LID_TEMP': 'M140',
		printMCodeReply("SET_LID_TEMP not implemented\r\n");
		return true;
	}
	if (StartWith(cmdIn, "M141 "))
	{
		//    'GET_LID_TEMP': 'M141',
		printMCodeReply("GET_LID_TEMP not implemented\r\n");
		return true;
	}
	if (StartWith(cmdIn, "M301 "))
	{
		//    'EDIT_PID_PARAMS': 'M301',
		printMCodeReply("EDIT_PID_PARAMS not implemented\r\n");
		return true;
	}
	if (StartWith(cmdIn, "M56626 "))
	{
		//    'SET_RAMP_RATE': 'M566',
		printMCodeReply("SET_RAMP_RATE not implemented\r\n");
		return true;
	}
	if (StartWith(cmdIn, "M108 "))
	{
		//    'DEACTIVATE_LID': 'M108',
		printMCodeReply("DEACTIVATE_LID not implemented\r\n");
		return true;
	}
	if (StartWith(cmdIn, "M14 "))
	{
		//    'DEACTIVATE_BLOCK': 'M14',
		printMCodeReply("DEACTIVATE_BLOCK not implemented\r\n");
		return true;
	}
	if (StartWith(cmdIn, "M115 "))
	{
		//    'DEVICE_INFO': 'M115'
		printDeviceInfo();
		return true;
	}
	return false;
}

void printMCodeReply(const char* conetnt)
{
	Serial.print(conetnt);
	Serial.flush();
}

void decodeSetPlateTemp(const char* cmd)
{
	if (StartWith(cmd, "heat"))
	{
		GlobalVars::ctrlPlan.tempCtrlMode = 1;
		TempCtrlService::initTempCtrlHeatUp();
	}
	else if (StartWith(cmd, "cool"))
	{
		GlobalVars::ctrlPlan.tempCtrlMode = -1;
		TempCtrlService::initTempCtrlCoolDown();
	}
	else
	{
		GlobalVars::ctrlPlan.tempCtrlMode = 0;
	}
	// M104 heat,35,1,10
	// M104 cool,10,2,0\r\n
	char* tgtStr = strchr(cmd, ',');
	if (tgtStr != NULL)
	{
		float tempTarget = atof(tgtStr + 1);
		char* rampRateStr = strchr(tgtStr + 2, ',');
		if (rampRateStr != NULL)
		{
			float rampRate = atof(rampRateStr + 1);
			char* holdSecsStr = strchr(rampRateStr + 2, ',');
			if (holdSecsStr != NULL)
			{
				unsigned long holdS = atol(holdSecsStr + 1);
				if (tempTarget >= -4 && tempTarget <= max(GlobalVars::containerTemp.warningThreshold, GlobalVars::plateTemp.warningThreshold))
				{
					GlobalVars::containerTemp.targetVal = tempTarget;
					if (GlobalVars::flagSimuRate != 0) {
						GlobalVars::plateTemp.targetVal = tempTarget;
						GlobalVars::sampleLeftTemp.targetVal = tempTarget;
						GlobalVars::sampleRightTemp.targetVal = tempTarget;
						GlobalVars::pumpTemp.targetVal = (tempTarget + GlobalVars::surroundingTemp.targetVal) / 2;
					}
					GlobalVars::ctrlPlan.tempFeedbackMode = 2;
					GlobalVars::ctrlPlan.tempCtrlRampRate = rampRate;
					GlobalVars::ctrlPlan.flagStartTempChange = 0;
					GlobalVars::ctrlPlan.targetHoldDurMs = holdS * 1000;
					GlobalVars::ctrlPlan.targetHoldStartMs = 0;

					GlobalVars::restartCurves();

					sprintf(GlobalVars::outputBuffer, "{%s} towards {%.1f} at {%.1f} dps, then hold for {%d}s\r\n",
						GlobalVars::controlMode2char(GlobalVars::ctrlPlan.tempFeedbackMode, GlobalVars::ctrlPlan.tempCtrlMode),
						GlobalVars::containerTemp.targetVal,
						GlobalVars::ctrlPlan.tempCtrlRampRate,
						(int)holdS);
					printMCodeReply(GlobalVars::outputBuffer);

#ifdef THERMAL_SOURCE_TEC
					PowerDrive::setChannelPercent(FAN_CONTROL_CHANNEL, 100);
#endif
				}
				else {
					Serial.println("Invalid temperature range");
				}
			}
		}
	}

	if (GlobalVars::ctrlPlan.tempFeedbackMode >= 2)
	{
		Report::setClipStart();
		PowerDrive::enable();
	}
}

void processDeactivateAll()
{
	PowerDrive::disable();
	Report::setClipEnd();
	PowerDrive::setChannelPercent(Heat, All, 0);
	PowerDrive::setChannelPercent(Cool, All, 0);
	GlobalVars::ctrlPlan.tempFeedbackMode = 0;
	GlobalVars::ctrlPlan.tempCtrlMode = 0;
	LedControl::unlockModes();
	printMCodeReply("All off\r\n");
}

void printDeviceInfo()
{
	printMCodeReply("");
}